#include "myantenna_laser.hpp"
#include <iomanip>

#ifndef MYANTENNA_LASER_USE_CONFIG_H
#define MYANTENNA_LASER_NAME ("myantenna_laser")

#define MYANTENNA_LASER_RS485_PATH ("/dev/ttyUSB0")
#define MYANTENNA_LASER_RS485_BAUDRATE (115200)
#define MYANTENNA_LASER_RS485_DE_PATH ("/dev/gpiochip3")
#define MYANTENNA_LASER_RS485_DE_PIN (24)
#define MYANTENNA_LASER_RS485_FRAME_TIMEOUT_MS (2000)

#define MYANTENNA_LASER_MODBUS_SLAVE_ADDR (0x01)
#endif // !MYANTENNA_LASER_USE_CONFIG_H

uart_v2 *Pmyantenna_laser_uart = NULL;
rs485_v3 *Pmyantenna_laser_rs485 = NULL;
agile_modbus_rtu_master_simple *Pmyantenna_laser_rtu_master = NULL;

static yj_return init_flag = YJ_ERROR;

int myantenna_laser_rs485_modbus_read(uint8_t *data)
{
    if (!Pmyantenna_laser_rs485 || !Pmyantenna_laser_rtu_master || !Pmyantenna_laser_uart || !data)
        return -1;
    return Pmyantenna_laser_rs485->rs485_recv(data, MAX_ADU_LENGTH);
}

int myantenna_laser_rs485_modbus_write(uint8_t *data, uint32_t len)
{
    if (!Pmyantenna_laser_rs485 || !Pmyantenna_laser_rtu_master || !Pmyantenna_laser_uart || !data)
        return -1;
    return Pmyantenna_laser_rs485->rs485_send(data, len);
}

yj_return myantenna_laser_init(void)
{
    std::string name(MYANTENNA_LASER_NAME);
    Pmyantenna_laser_uart = new uart_v2(MYANTENNA_LASER_RS485_PATH, MYANTENNA_LASER_RS485_BAUDRATE, 8, 1);
    Pmyantenna_laser_rs485 = new rs485_v3(Pmyantenna_laser_uart,  MYANTENNA_LASER_RS485_FRAME_TIMEOUT_MS);
    // Pmyantenna_laser_rs485 = new rs485_v3(Pmyantenna_laser_uart, MYANTENNA_LASER_RS485_DE_PATH, MYANTENNA_LASER_RS485_DE_PIN, MYANTENNA_LASER_RS485_FRAME_TIMEOUT_MS);
    Pmyantenna_laser_rtu_master = new agile_modbus_rtu_master_simple(name,
                                                                     myantenna_laser_rs485_modbus_write,
                                                                     myantenna_laser_rs485_modbus_read);

    if (Pmyantenna_laser_rs485->rs485_open() != YJ_OK)
        return init_flag;
    if (Pmyantenna_laser_rtu_master->init() != 0)
        return init_flag;
    init_flag = YJ_OK;
    return init_flag;
}

void myantenna_laser_deinit(void)
{
    if (Pmyantenna_laser_rs485)
    {
        Pmyantenna_laser_rs485->rs485_close();
        delete Pmyantenna_laser_rs485;
        Pmyantenna_laser_rs485 = NULL;
    }
    if (Pmyantenna_laser_rtu_master)
    {
        delete Pmyantenna_laser_rtu_master;
        Pmyantenna_laser_rtu_master = NULL;
    }
    if (Pmyantenna_laser_uart)
    {
        delete Pmyantenna_laser_uart;
        Pmyantenna_laser_uart = NULL;
    }
}

yj_return myantenna_laser_read_all(myantenna_laser_data_t *data)
{
    std::vector<uint16_t> dataTorecv(2);

    if (init_flag == YJ_ERROR || !data)
    {
        LOG(ERROR) << "❌ myantenna_laser_read_all init_flag error or data is null";

        return YJ_ERROR;
    }
    std::unique_lock<std::mutex> lock(Pmyantenna_laser_rs485->rs485_get_mutex());
    if (Pmyantenna_laser_rtu_master->read_holding_register_multiple(MYANTENNA_LASER_MODBUS_SLAVE_ADDR, 0x0F, dataTorecv) < 0)
    {
        LOG(ERROR) << "❌ myantenna_laser_read_all read holding regs error";
        return YJ_ERROR;
    }
    lock.unlock();
    

    data->distance = dataTorecv[0] << 16 | dataTorecv[1];

    return YJ_OK;
}

void myantenna_laser_print_data(myantenna_laser_data_t *data)
{
    if (!data)
    {
        LOG(ERROR) << "myantenna_laser_print_data data is null";
        return;
    }

    LOG(INFO) << "\n============= data start =============\n"
              << "distance: " << data->distance << "mm\n"
              << "============= data end =============\n";
}